#include "distance_field_grid_2.h"

namespace Field_packing
{
	Distance_field_grid_2::Distance_field_grid_2(const Container_box& container, size_t nb_rows, size_t nb_cols)
		: grid(container.x_min(), container.x_max(), container.y_min(), container.y_max(), nb_rows, nb_cols)
	{
		for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				grid.at(r, c).x = grid.xmin() + c*grid.xstep();
				grid.at(r, c).y = grid.ymin() + r*grid.ystep();
			}
		}
	}
	Distance_field_grid_2::~Distance_field_grid_2()
	{
		clear();
	}
	void Distance_field_grid_2::clear()
	{
		grid.for_each_node(std::mem_fun_ref(&DistanceNode::clear));
		for (size_t i = 0; i < local_fields.size(); i++)
			delete local_fields[i];
		local_fields.clear();
	}
	void Distance_field_grid_2::append_distance_field(const Packing_polygon& polygon)
	{
		max_dist = 0.0;
		//double inside_indicator = -10*std::max(grid.xstep(), grid.ystep());
		for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				//Point_2 p(grid.xmin()+c*grid.xstep(), grid.ymin()+r*grid.ystep());
				Point_2 p(grid.at(r, c).x, grid.at(r, c).y);
				Vector_2 v;
				Point_2 closest_pnt;
				double d = polygon.squared_distance(p, v, closest_pnt);
				d = std::sqrt(d);
				max_dist = std::max(max_dist, d);
				if (polygon.has_on_unbounded_side(p))
					grid.at(r, c).dist_polygon.push_back(d);
				else
					grid.at(r, c).dist_polygon.push_back(-d/*inside_indicator*/);
				grid.at(r, c).gradients.push_back(v);
				grid.at(r, c).closest_pnts.push_back(closest_pnt);
			}
		}
		// construct local frames
		Point_2 c = polygon.centroid();
		Local_frame_2 lf(Vector_2(1.0, 0.0), Vector_2(0.0, 1.0), c);
		local_frames.push_back(lf);

		// construct local coordinates
		double grid_width = grid.xmax() - grid.xmin(), grid_height = grid.ymax() - grid.ymin();
		const int range = 1;
		local_fields.push_back(new Grid_2<LocalFieldNode>(-range*grid_width/2, range*grid_width/2, -range*grid_height/2, range*grid_height/2, grid.nb_rows()*range, grid.nb_columns()*range));
		Grid_2<LocalFieldNode>& local_grid = *local_fields.back();
		Packing_polygon local_polygon ; // polygon in its local coordinate
		for (Packing_polygon::Vertex_const_iterator vit = polygon.vertices_begin(); vit != polygon.vertices_end(); ++vit)
			local_polygon.push_back(lf.local_coord(*vit));

#ifdef _CILK_
#pragma message("Using Intel Cilk plus")
		cilk_for (size_t r = 0; r <= local_grid.nb_rows(); r++)
#else
		for (size_t r = 0; r <= local_grid.nb_rows(); r++)
#endif
		{
			for (size_t c = 0; c <= local_grid.nb_columns(); c++)
			{
				Point_2 p(local_grid.xmin()+c*local_grid.xstep(), local_grid.ymin()+r*local_grid.ystep());
				double d = local_polygon.squared_distance(p, local_grid.at(r,c).grad, local_grid.at(r,c).closest_pnt);
				d = std::sqrt(d);
				if (local_polygon.has_on_unbounded_side(p))
					local_grid.at(r, c).dist = d;
				else
					local_grid.at(r, c).dist = -d/*inside_indicator*/;
			}
		}
	}

	double Distance_field_grid_2::local_query(const Point_2& p, size_t polygon_id, Vector_2& grad, Point_2& closest_point) const
	{
		const Grid_2<LocalFieldNode>& g = *local_fields[polygon_id];
		if (p.x() <= g.xmin() || p.y() <= g.ymin() || p.x() >= g.xmax() || p.y() >= g.ymax() )
		{
			double w = g.xmax() - g.xmin();
			double h = g.ymax() - g.ymin();
			// TODO: it should not be arbitrarily zero vector
			grad = Vector_2(0.0, 0.0);
			closest_point = Point_2(0.0, 0.0); //set as the centroid of the polygon
			return std::sqrt(w*w + h*h);
		}
		size_t c = std::floor((p.x() - g.xmin())/g.xstep());
		size_t r = std::floor((p.y() - g.ymin())/g.ystep());
		double frac_c = p.x() - (g.xmin() + c*g.xstep());
		double frac_r = p.y() - (g.ymin() + r*g.ystep());
		double w[4] = {frac_c*frac_r, (g.xstep()-frac_c)*(g.ystep()-frac_r), frac_c*(g.ystep()-frac_r), (g.xstep()-frac_c)*frac_r};
		const LocalFieldNode *n[4] = {&g.at(r+1, c+1), &g.at(r, c), &g.at(r, c+1), &g.at(r+1, c)};
		double cell_area = g.xstep()*g.ystep();
		double d = w[0]*n[0]->dist + w[1]*n[1]->dist + w[2]*n[2]->dist + w[3]*n[3]->dist;
		d /= cell_area;
		grad = w[0]*n[0]->grad + w[1]*n[1]->grad + w[2]*n[2]->grad + w[3]*n[3]->grad;
		grad = grad / cell_area;
		Vector_2 cp = w[0]*Vector_2(CGAL::ORIGIN, n[0]->closest_pnt) + w[1]*Vector_2(CGAL::ORIGIN, n[1]->closest_pnt);
		cp = cp + w[2]*Vector_2(CGAL::ORIGIN, n[2]->closest_pnt) + w[3]*Vector_2(CGAL::ORIGIN, n[3]->closest_pnt);
		cp = cp / cell_area;
		closest_point = CGAL::ORIGIN + cp;
		return d;
	}

	double Distance_field_grid_2::distance_query(const Point_2& p, size_t polygon_id) const
	{
		if (p.x() <= grid.xmin() || p.y() <= grid.ymin() || p.x() >= grid.xmax() || p.y() >= grid.ymax() )
		{
			//Vector_2 v;
			//Point_2 p;
			//double d = local_query(local_frames[polygon_id].local_coord(p), polygon_id, v, p);
			double d = std::sqrt(CGAL::squared_distance(p, local_frames[polygon_id].o));
			return d;
		}
		size_t c = std::floor((p.x() - grid.xmin())/grid.xstep());
		size_t r = std::floor((p.y() - grid.ymin())/grid.ystep());
		double frac_c = p.x() - (grid.xmin() + c*grid.xstep());
		double frac_r = p.y() - (grid.ymin() + r*grid.ystep());
		double w[4] = {frac_c*frac_r, (grid.xstep()-frac_c)*(grid.ystep()-frac_r), frac_c*(grid.ystep()-frac_r), (grid.xstep()-frac_c)*frac_r};
		double d = w[0]*grid.at(r+1, c+1).dist_polygon[polygon_id] + w[1]*grid.at(r, c).dist_polygon[polygon_id];
		d += w[2]*grid.at(r, c+1).dist_polygon[polygon_id]+w[3]*grid.at(r+1, c).dist_polygon[polygon_id];
		return d/(grid.xstep()*grid.ystep());
		//Point_2 lp = local_frames[polygon_id].local_coord(p);
		//Point_2 lcp;
		//Vector_2 lgrad;
		//double d = local_query(lp, polygon_id, lgrad, lcp);
		//return d;
	}

	double Distance_field_grid_2::gradient_query(const Point_2& p, size_t polygon_id, Vector_2& grad, Point_2& closest_point) const
	{
		if (p.x() <= grid.xmin() || p.y() <= grid.ymin() || p.x() >= grid.xmax() || p.y() >= grid.ymax() )
		{
			double d = local_query(local_frames[polygon_id].local_coord(p), polygon_id, grad, closest_point);
			grad = local_frames[polygon_id].world_coord(grad);
			closest_point = local_frames[polygon_id].world_coord(closest_point);
			return d;
		}
		size_t c = std::floor((p.x() - grid.xmin())/grid.xstep());
		size_t r = std::floor((p.y() - grid.ymin())/grid.ystep());
		double frac_c = p.x() - (grid.xmin() + c*grid.xstep());
		double frac_r = p.y() - (grid.ymin() + r*grid.ystep());
		double cell_area = grid.xstep()*grid.ystep();
		double w[4] = {frac_c*frac_r, (grid.xstep()-frac_c)*(grid.ystep()-frac_r), frac_c*(grid.ystep()-frac_r), (grid.xstep()-frac_c)*frac_r};

		double d = w[0]*grid.at(r+1, c+1).dist_polygon[polygon_id] + w[1]*grid.at(r, c).dist_polygon[polygon_id];
		d += w[2]*grid.at(r, c+1).dist_polygon[polygon_id] + w[3]*grid.at(r+1, c).dist_polygon[polygon_id];
		d /= cell_area;

		grad = w[0]*grid.at(r+1, c+1).gradients[polygon_id] + w[1]*grid.at(r, c).gradients[polygon_id];
		grad = grad + w[2]*grid.at(r, c+1).gradients[polygon_id]+w[3]*grid.at(r+1, c).gradients[polygon_id];
		grad = grad / cell_area;

		Vector_2 cp = w[0]*Vector_2(CGAL::ORIGIN, grid.at(r+1, c+1).closest_pnts[polygon_id]) + w[1]*Vector_2(CGAL::ORIGIN, grid.at(r, c).closest_pnts[polygon_id]);
		cp = cp + w[2]*Vector_2(CGAL::ORIGIN, grid.at(r, c+1).closest_pnts[polygon_id]) + w[3]*Vector_2(CGAL::ORIGIN, grid.at(r+1, c).closest_pnts[polygon_id]);
		cp = cp / cell_area;
		closest_point = CGAL::ORIGIN + cp;
		//Point_2 lp = local_frames[polygon_id].local_coord(p);
		//Point_2 lcp;
		//Vector_2 lgrad;
		//double d = local_query(lp, polygon_id, lgrad, lcp);
		//grad = local_frames[polygon_id].world_coord(lgrad);
		//closest_point = local_frames[polygon_id].world_coord(lcp);
		return d;
	}

	void Distance_field_grid_2::update_distance_field(size_t polygon_id, const Transformation_2& transformation)
	{
		local_frames[polygon_id].transform(transformation);

		// update the field energy at each grid node after the polygon transformed, in an incremental way
		// from left bottom corner (r = 0, c = 0)
		// coordinate in the new local frame
		Vector_2 t_xstep(grid.xstep()*local_frames[polygon_id].u.x(), grid.xstep()*local_frames[polygon_id].v.x());
		Vector_2 t_ystep(grid.ystep()*local_frames[polygon_id].u.y(), grid.ystep()*local_frames[polygon_id].v.y());
		Point_2 y_base_pnt = local_frames[polygon_id].local_coord(Point_2(grid.at(0, 0).x, grid.at(0, 0).y));

#ifdef _CILK_
		cilk_for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			Point_2 x_base_pnt = y_base_pnt + r*t_ystep;
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				Vector_2 v;
				Point_2 closest_pnt;
				grid.at(r, c).dist_polygon[polygon_id] = local_query(x_base_pnt, polygon_id, v, closest_pnt);
				grid.at(r, c).gradients[polygon_id] = local_frames[polygon_id].world_coord(v);
				grid.at(r, c).closest_pnts[polygon_id] = local_frames[polygon_id].world_coord(closest_pnt);
				x_base_pnt = x_base_pnt + t_xstep;				
			}
		}
#else
		for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			Point_2 x_base_pnt = y_base_pnt;
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				Vector_2 v;
				Point_2 closest_pnt;
				grid.at(r, c).dist_polygon[polygon_id] = local_query(x_base_pnt, polygon_id, v, closest_pnt);
				grid.at(r, c).gradients[polygon_id] = local_frames[polygon_id].world_coord(v);
				grid.at(r, c).closest_pnts[polygon_id] = local_frames[polygon_id].world_coord(closest_pnt);
				x_base_pnt = x_base_pnt + t_xstep;
			}
			y_base_pnt = y_base_pnt + t_ystep;
		}
#endif
	}

	void Distance_field_grid_2::gl_draw(size_t polygon_id) const
	{
		double min_dist = 0.0, mean_dist = (min_dist + max_dist)/2;
		glBegin(GL_QUADS);
		for (size_t r = 0; r < grid.nb_rows(); r++)
		{
			for (size_t c = 0; c < grid.nb_columns(); c++)
			{
				std::pair<size_t, size_t> idx[] = { std::make_pair(r, c), std::make_pair(r, c+1),
													std::make_pair(r+1, c+1),	std::make_pair(r+1, c) };
				for (int i = 0; i < 4; i++)
				{
					const DistanceNode& node = grid.at(idx[i].first, idx[i].second);
					double d = node.dist_polygon.at(polygon_id);
					GLfloat color[3] = {0.0f, 0.0f, 0.0f}; // RGB
					if (d >= max_dist)	
						color[0] = 1.0f;
					else if (d < mean_dist)
					{
						color[1] = (d - min_dist)/(mean_dist - min_dist);
						color[2] = (mean_dist - d)/(mean_dist - min_dist);
					}
					else
					{
						color[0] = (d - mean_dist)/(max_dist - mean_dist);
						color[1] = (max_dist - d)/(max_dist - mean_dist);
					}
					glColor3fv(color);
					glVertex2d(node.x, node.y);
				}
			}
		}
		glEnd();
		// draw the gradient 
		glColor3f(0.0, 0.0, 0.0);
		glLineWidth(1.5f);
		glBegin(GL_LINES);
		for (size_t r = 0; r <= grid.nb_rows(); r++)
		{
			for (size_t c = 0; c <= grid.nb_columns(); c++)
			{
				const DistanceNode& node = grid.at(r, c);
				Vector_2 v = node.gradients[polygon_id];
				v = v/CGAL::sqrt(v.squared_length());
				glVertex2d(node.x, node.y);
				glVertex2d(node.x + v.x(), node.y + v.y());
			}
		}
		glEnd();
	}

	void Distance_field_grid_2::gl_draw_local_field(size_t polygon_id) const
	{
		const Grid_2<LocalFieldNode>& lg = *local_fields[polygon_id];
		double local_max_dist = lg.at(0, 0).dist;
		local_max_dist = std::max(lg.at(lg.nb_rows(), 0).dist, local_max_dist);
		local_max_dist = std::max(lg.at(0, lg.nb_columns()).dist, local_max_dist);
		local_max_dist = std::max(lg.at(lg.nb_rows(), lg.nb_columns()).dist, local_max_dist);
		double mean_dist = local_max_dist/2;
		Vector_2 iu(local_frames[polygon_id].u.x(), local_frames[polygon_id].v.x());
		Vector_2 iv(local_frames[polygon_id].u.y(), local_frames[polygon_id].v.y());
		glBegin(GL_QUADS);
		for (size_t r = 0; r < lg.nb_rows(); r++)
		{
			for (size_t c = 0; c < lg.nb_columns(); c++)
			{
				std::pair<size_t, size_t> idx[] = { std::make_pair(r, c), std::make_pair(r, c+1),
													std::make_pair(r+1, c+1),	std::make_pair(r+1, c) };
				for (int i = 0; i < 4; i++)
				{
					//double node = grid.at(idx[i].first, idx[i].second);
					double d = lg.at(idx[i].first, idx[i].second).dist;
					GLfloat color[3] = {0.0f, 0.0f, 0.0f}; // RGB
					if (d >= local_max_dist)	
						color[0] = 1.0f;
					else if (d < mean_dist)
					{
						color[1] = (d - 0.0)/(mean_dist - 0.0);
						color[2] = (mean_dist - d)/(mean_dist - 0.0);
					}
					else
					{
						color[0] = (d - mean_dist)/(local_max_dist - mean_dist);
						color[1] = (local_max_dist - d)/(local_max_dist - mean_dist);
					}
					glColor3fv(color);
					Vector_2 op(CGAL::ORIGIN, Point_2(lg.xmin()+idx[i].second*lg.xstep(), lg.ymin()+idx[i].first*lg.ystep()));
					glVertex2d(op*iu+local_frames[polygon_id].o.x(), op*iv+local_frames[polygon_id].o.y());
				}
			}
		}
		glEnd();
	}
}